#include "Mahony.h"
#include "math.h"
// Definitions
#define sampleFreq	166.6f			// sample frequency in Hz
#define twoKpDef	(2.0f * 50.0f)	// 2 * proportional gain
#define twoKiDef	(2.0f * 0.1f)	// 2 * integral gain
// Variable definitions
volatile float twoKp = twoKpDef;											// 2 * proportional gain (Kp)
volatile float twoKi = twoKiDef;											// 2 * integral gain (Ki)
volatile float integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f;	// integral error terms scaled by Ki
float pitch,yaw,roll;
// Function declarations
float invSqrt(float x);
void MahonyAHRSupdateIMU(float q[4], float gx, float gy, float gz, float ax, float ay, float az) 
{
	float recipNorm;
	float halfvx, halfvy, halfvz;
	float halfex, halfey, halfez;
	float qa, qb, qc;
	if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) 
    {
		recipNorm = invSqrt(ax * ax + ay * ay + az * az);
		ax *= recipNorm;
		ay *= recipNorm;
		az *= recipNorm;        
		halfvx = q[1] * q[3] - q[0] * q[2];
		halfvy = q[0] * q[1] + q[2] * q[3];
		halfvz = q[0] * q[0] - 0.5f + q[3] * q[3];
		halfex = (ay * halfvz - az * halfvy);
		halfey = (az * halfvx - ax * halfvz);
		halfez = (ax * halfvy - ay * halfvx);
		if(twoKi > 0.0f) 
    {
			integralFBx += twoKi * halfex * (1.0f / sampleFreq);	
			integralFBy += twoKi * halfey * (1.0f / sampleFreq);
			integralFBz += twoKi * halfez * (1.0f / sampleFreq);
			gx += integralFBx;	
			gy += integralFBy;
			gz += integralFBz;
		}
		else 
    {
			integralFBx = 0.0f;	
			integralFBy = 0.0f;
			integralFBz = 0.0f;
		}
		gx += twoKp * halfex;
		gy += twoKp * halfey;
		gz += twoKp * halfez;
	}
	gx *= (0.5f * (1.0f / sampleFreq));	
	gy *= (0.5f * (1.0f / sampleFreq));
	gz *= (0.5f * (1.0f / sampleFreq));
	qa = q[0];
	qb = q[1];
	qc = q[2];
	q[0] += (-qb * gx - qc * gy - q[3] * gz); 
	q[1] += (qa * gx + qc * gz - q[3] * gy);
	q[2] += (qa * gy - qb * gz + q[3] * gx);
	q[3] += (qa * gz + qb * gy - qc * gx); 
	recipNorm = invSqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
	q[0] *= recipNorm;
	q[1] *= recipNorm;
	q[2] *= recipNorm;
	q[3] *= recipNorm;
}
float invSqrt(float x) 
{
	float halfx = 0.5f * x;
	float y = x;
	long i = *(long*)&y;
	i = 0x5f3759df - (i>>1);
	y = *(float*)&i;
	y = y * (1.5f - (halfx * y * y));
	return y;
}
void q4_Init(float ax,float ay,float az)
{
	pitch=atan2f(ax,az);
	roll=atan2f(ay,az);
	q[0]=cos(pitch/2)*cos(roll/2);
	q[1]=sin(pitch/2)*cos(roll/2);
	q[2]=cos(pitch/2)*sin(roll/2);
	q[3]=-sin(pitch/2)*sin(roll/2);
}
void Get_EulerAngle()
{
	pitch=(asin(2*(q[0]*q[2]-q[1]*q[3])))/6.28*360;
	yaw=(atan2(2*(q[0]*q[3]+q[1]*q[2]),(1-2*(q[2]*q[2]+q[3]*q[3]))))/6.28*360;
	roll=(atan2(2*(q[0]*q[1]+q[2]*q[3]),(1-2*(q[1]*q[1]+q[2]*q[2]))))/6.28*360;
}	